#include "App_Communication.h"

uint8_t rx_buffer[TX_PLOAD_WIDTH] = {0};

Com_Type App_Communication_ReceiveData(Remote_Struct *remote_data)
{
    // 1.接收数据
    uint8_t result = Int_SI24R1_RxPacket(rx_buffer);
    if (result == 1) {
        return Com_FAIL;
    }

    // 2.校验数据
    // 2.1帧头校验
    if ((rx_buffer[0] != FRAME0) || (rx_buffer[1] != FRAME1) || (rx_buffer[2] != FRAME2)) {
        return Com_FAIL;
    }
    // 2.2 校验和
    uint32_t check_sum = 0;
    for (uint8_t i = 0; i < 13; i++) {
        check_sum += rx_buffer[i];
    }
    uint32_t check_sum_rec = rx_buffer[13] << 24 | rx_buffer[14] << 16 | rx_buffer[15] << 8 | rx_buffer[16];
    if (check_sum != check_sum_rec) {
        return Com_FAIL;
    }

    // 3.解析数据
    remote_data->THR        = rx_buffer[3] << 8 | rx_buffer[4];
    remote_data->YAW        = rx_buffer[5] << 8 | rx_buffer[6];
    remote_data->PIT        = rx_buffer[7] << 8 | rx_buffer[8];
    remote_data->ROL        = rx_buffer[9] << 8 | rx_buffer[10];
    remote_data->fix_height = rx_buffer[11];
    remote_data->power_down = rx_buffer[12];

    return Com_SUCCESS;
}

/**
 * @brief 处理遥控器连接状态:收到一个数据包，认为连接成功;200ms都没有收到数据包，认为断开连接
 * @param isReceived 是否收到数据包
 * @param remote_status 遥控器连接状态
 */
uint32_t last_receive_tick = 0;
void App_Communication_HandleRemoteStatus(Com_Type isReceived, Remote_Status *remote_status)
{
    if (isReceived == Com_SUCCESS) {
        *remote_status    = CONNECTED;
        last_receive_tick = xTaskGetTickCount();
    } else {
        if (xTaskGetTickCount() - last_receive_tick > 200) {
            *remote_status = UNCONNECTED;
        }
    }
}

// 油门状态
THR_Status thr_status   = FREE;
uint32_t max_start_tick = 0;
uint32_t min_start_tick = 0;
/**
 * @brief 检测解锁信号: 油门最高保持1s -> 油门最低保持1s -> 解锁
 */
Com_Type App_Communication_CheckUnlock(Remote_Struct *remote_data)
{
    switch (thr_status) {
        case FREE:
            if (remote_data->THR >= 900) {
                thr_status     = MAX;
                max_start_tick = xTaskGetTickCount();
            }
            break;
        case MAX:
            if (remote_data->THR < 900) {
                if (xTaskGetTickCount() - max_start_tick < 1000) {
                    thr_status = FREE;
                } else {
                    thr_status = LEAVE_MAX;
                }
            }
            break;
        case LEAVE_MAX:
            if (remote_data->THR <= 100) {
                thr_status     = MIN;
                min_start_tick = xTaskGetTickCount();
            }
            break;
        case MIN:
            if (xTaskGetTickCount() - min_start_tick < 1000) {
                if (remote_data->THR > 100) {
                    thr_status = FREE;
                }
            } else {
                thr_status = UNLOCK;
            }
            break;
        case UNLOCK:
            return Com_SUCCESS;
        default:
            break;
    }
    return Com_FAIL;
}

/**
 * @brief 处理飞机的工作状态
 * @param remote_status 遥控连接状态
 * @param remote_data 遥控数据
 * @param flight_status 飞机的工作状态
 */
extern uint16_t height;   //实时高度
extern uint16_t fix_height ; //固定高度
extern float static_accel_z; //Z轴加速度
void App_Communication_HandleFlightStatus(Remote_Status remote_status, Remote_Struct *remote_data, Flight_Status *flight_status)
{
    switch (*flight_status) {
        case IDEL:
            if (remote_status == CONNECTED) {
                Com_Type isUnlock = App_Communication_CheckUnlock(remote_data);
                if (isUnlock == Com_SUCCESS) {
                    *flight_status = NORMAL;
                    // 计算初始的垂直方向的加速度
                    static_accel_z = Com_IMU_GetNormAccZ();

                }
            }
            break;
        case NORMAL:
            if (remote_status == CONNECTED) {
                if (remote_data->fix_height == 1) {
                    *flight_status = FIX_HEIGHT;
                    // 记录当前高度
                   fix_height = height;
                }
            } else {
                *flight_status = FAULT;
            }
            break;
        case FIX_HEIGHT:
            if (remote_status == CONNECTED) {
                if (remote_data->fix_height == 1) {
                    *flight_status = NORMAL;
                }
            } else {
                *flight_status = FAULT;
            }
            break;
        case FAULT:
            // 等待任务通知
            ulTaskNotifyTake(pdTRUE, 2000);
            *flight_status = IDEL;
            thr_status     = FREE;
            break;
        default:
            break;
    }
}
